;;;
;;; Suty: String Operation Utilities
;;; 
BITS 64
;;; 
;;;  LICENSE AGREEMENT:
;;; 
;;;  You agree that we don't agree to anything.
;;; 
;;; 
;;;  DISCLAIMER OF WARRANTIES:
;;; 
;;;  This software is provided to you "AS IS," and the authors and
;;;  contributors disclaim any warranty or liability obligations to you
;;;  of any kind, implied or express, including but not limited to the
;;;  warranties of merchantability, fitness for a particular purpose,
;;;  and non-infringement.
;;; 
section .data align=16
funity dq 1.0, 1.0
;; floating point threshold 
eps dq 1.0e-16, 1.0e-16
fth dq 1.0e-16, 1.0e-16
errno dq 0
mxcsrd dd 0
;;; 
section .text align=16
%assign rc   0x04
%assign arg1 0x18
%assign arg2 0x20
%assign arg3 0x28
%assign arg4 0x30
;;; 
global cpmems1_, cpmems2_, cpmems4_, cpmems8_
global cpmemf1_, cpmemf2_, cpmemf4_, cpmemf8_
global clemem1_, clemem2_, clemem4_, clemem8_
global initsuty_
global muldivpd_
global divpd_
global kthsmallest_
global cpuida_, cpuidb_, cpuidc_, cpuidd_
global fxsave_, setdaz_,
global bsdshmget_, bsdshmat_, bsdshmdt_, bsdshmctl_
global bsdstat_
global geterrnoa_
;;; 
initsuty_:
	push rbp
	mov rbp, rsp
	;;	
	mov rax, [rdi]
	mov [eps], rax
	mov [eps+8],rax
	;; 
	mov rax, [rsi]
	mov [fth], rax
	mov [fth+8],rax
	;; 
	pop rbp
	ret

%macro cpmem 1
	push rbp
	mov rbp, rsp
	;; 
	mov rcx, rdx
	cld
%if %1 == 2
	rep movsw
%elif %1 == 4
	rep movsd
%elif %1 == 8
	rep movsq
%elif %1 == 1
	rep movsb
%endif
	mov rax, rdi		; <<<
	;; 
	pop rbp
	ret
%endm

%macro clemem 1
	push rbp
	mov rbp, rsp
	;; 
	mov rcx, rsi
	xor rax, rax
	cld
%if %1 == 2
	rep stosw
%elif %1 == 4
	rep stosd
%elif %1 == 8
	rep stosq
%elif %1 == 1
	rep stosb
%endif
	;; 
	pop rbp
	ret
%endm

;;;
cpmems1_:
cpmemf1_:	
cpmem 1
cpmems2_:
cpmemf2_:
cpmem 2	
cpmems4_:
cpmemf4_:
cpmem 4
cpmems8_:
cpmemf8_:
cpmem 8
clemem1_:
clemem 1
clemem2_:
clemem 2
clemem4_:
clemem 4
clemem8_:
clemem 8

%macro divpdm 1
%if %1 == 0
 	movapd xmm0, [rdi]	; xmm0 = a
	movapd xmm1, [rsi]	; xmm1 = c
%endif	
	;; where a<eps a=1, c=0
	movapd xmm6, xmm0
	cmppd xmm6, [eps], 1	; xmm6=(a<eps)
	movapd xmm5, xmm6	; xmm5=(a<eps)
 	andnpd xmm6, xmm1	; where a<eps c=0
	andpd xmm5, [funity]	; where a<eps xmm5=1.0
	addpd xmm0, xmm5	; where a<eps a=1.0, avoid 0div
	movapd xmm1, xmm6	; xmm1 = c
	;; 
 	divpd xmm1, xmm0	; xmm0 = c/a
%endm
	
	%macro muldivpdm 1
%if %1 == 0
 	movapd xmm4, [rdi]	; xmm4 = a
 	movapd xmm0, [rsi]	; xmm0 = b
	movapd xmm1, [rdx]	; xmm1 = c
%endif
	;; where a<floth a=0
	movapd xmm6, xmm4
	cmppd xmm6, [eps], 1	; xmm6=(a<eps)
	andnpd xmm6, xmm4	; where a<eps xmm6=0
	movapd xmm4, xmm6	;
	
	;; where c<fth c=1, a=0
	movapd xmm6, xmm1
	cmppd xmm6, [fth], 1	; xmm6=(c<fth)
	movapd xmm5, xmm6	; xmm5=(c<fth)
 	andnpd xmm6, xmm4	; where c<fth a=0
	andpd xmm5, [funity]	; where c<fth xmm5=1.0
	addpd xmm1, xmm5	; where c<fth c=1.0, avoid 0div
	movapd xmm4, xmm6	; xmm4 = a
	;; 
 	divpd xmm0, xmm1	; xmm0 = b/c
  	mulpd xmm0, xmm4	; xmm0 = a*b/c
%endm
	
;;; 
muldivpd_:
;$;subroutine muldivpd(a,b,c,n)
;$;	inteter,intent(in)::n
;$;	real*8,intent(inout)::a(n)
;$;	real*8,intent(in)::b(n),c(n)
;$;end subroutine
;;; 
	push rbp
	mov rbp, rsp
	;;
	mov rax, rcx
	shr rcx, 1
	test rcx, rcx
	jz .exit
	mov r8, rcx
	shl r8, 1
	sub rax, r8
	push rax
.do:

	muldivpdm 0
	movapd [rdi], xmm0	; a=b/c
	add rdi, 16		;
	add rsi, 16		;
	add rdx, 16		;
	dec rcx
	ja .do			;
	;; 
.next:
	pop rax
.exit:
	pop rbp
	ret

;;; 
divpd_:
;$;subroutine divpd(a,c,n)
;$;	inteter,intent(in)::n
;$;	real*8,intent(inout)::a(n)
;$;	real*8,intent(in)::c(n)
;$;end subroutine
;;; 
	push rbp
	mov rbp, rsp
	;;
	mov rcx, rdx
	shr rcx, 1
	test rcx, rcx
	jz .exit

.do:
	divpdm 0
	movapd [rdi], xmm1	; a=b/c
	add rdi, 16		;
	add rsi, 16		;
	dec rcx
	ja .do			;
	;; 
.exit:
	pop rbp
	ret
;;;

%macro swap 2
	push %1
	push %2
	pop %1
	pop %2
%endm

;;; subroutine kthsmallest(a,n,k)
;;; real*8,intent(inout)::a(n)
;;; integer,intent(in)::n
;;; integer,intent(in)::k	
kthsmallest_:
	push rbp
	mov rbp, rsp
	;;
	xor r9, r9		; =l
	mov r8, rsi		;
	dec r8			; =m=n-1
	dec rdx			; = k
	;; 
.main:
	cmp r9, r8
	jge .done
	
	mov rsi, [rdi+8*rdx]	; rsi= x = a(k)
	mov rax, r9		; =i
	mov rcx, r8		; =j=m
.do1:
	cmp rsi, [rdi+8*rax]	; x > a(i)
	jle .do2
	inc rax
	jmp .do1
.do2:
	cmp rsi, [rdi+8*rcx]	; x < a(j)
	jge .next1
	dec rcx
	jmp .do2
.next1:
	cmp rax, rcx		; i <= j
	jg .next4
.next2:
	je .next3
	swap qword [rdi+8*rax], qword [rdi+8*rcx]
.next3:
	inc rax
	dec rcx
	cmp rax, rcx		; i <= j
	jle .do1
.next4:
	cmp rcx, rdx		; j < k
	jge .sub1
	mov r9, rax 		; l = i
.sub1:
	cmp rdx, rax		; k < i
	jge .sub2
	mov r8, rcx 		; m = j
.sub2:
	jmp .main
.done:
.exit:
	pop rbp
	ret
;;; 
%macro cpuidm 1
	push rbp
	push rbx
	mov rbp, rsp
	;;
	mov eax, edi
	cpuid
	mov eax, %1
	;;
	pop rbx
	pop rbp
	ret
%endm
cpuida_:
cpuidm eax
cpuidb_:
cpuidm ebx
cpuidc_:
cpuidm ecx
cpuidd_:
cpuidm edx
;;;
fxsave_:	
	push rbp
	mov rbp, rsp
   	fxsave [rdi]
	pop rbp
	ret
;;;
setdaz_:	
	push rbp
	mov rbp, rsp
	;;
	stmxcsr [mxcsrd]
	mov eax, [mxcsrd]
	mov [rdi], eax
	or ax, 0940h		; DAZ,DM,UM
	mov [mxcsrd], eax
	ldmxcsr [mxcsrd]
	;;
	pop rbp
	ret
;;;
%assign ELAST 92
%macro return 0
	mov qword [errno], rax
	or rax, rax
	jz %%.exit
	cmp rax, ELAST	
	jbe %%.failure
%%.exit:
	pop rbp
	ret
%%.failure:
	mov [errno], rax
	mov rax, -1
	jmp %%.exit
%endmacro
;;;
%macro dosyscall 1
	push rbp
	mov rbp, rsp
	;;
	mov rax, %1
	syscall
	return
%endmacro
;;;
bsdshmget_:  dosyscall 231
bsdshmat_:   dosyscall 228
bsdshmdt_:   dosyscall 230
bsdshmctl_:  dosyscall 229
bsdfork_:    dosyscall 2
bsdwait4_:   dosyscall 7
bsdkill_:    dosyscall 37
bsdwrite_:   dosyscall 4
bsdopen_:    dosyscall 5
bsdclose_:   dosyscall 6
bsdlseek_:   dosyscall 19
bsdstat_:    dosyscall 278
bsdfaccess_: dosyscall 33
;;; 
geterrnoa_:
	push rbp
	mov rbp, rsp
	mov rax, [errno]
	pop rbp
	ret

